#include <memory>
#include <iostream>
#include <string>
#include <cstring>
#include <signal.h>
#include "can.hpp"

namespace CANalyst2
{
    #define SERVO_WORK_MODE_INDEX   0x6060 // 1 byte (int8_t)
    #define VALID_WORK_MODE_INDEX   0x6061 // 1 byte (int8_t)
    #define TARGET_POS_INDEX        0x607A // 4 byte (int32_t)
    #define ACTUAL_POS_INDEX        0x6063 // 4 byte (int32_t)
    #define ACTUAL_POS_INDEX_ZEROERR  0x6064 // 4 byte (int32_t)
    #define PROFILED_VELOCITY_INDEX 0x6081 // 4 byte (int32_t)
    #define PROFILED_ACC_INDEX      0x6083 // 4 byte (int32_t) 
    #define PROFILED_DEC_INDEX      0x6084 // 4 byte (int32_t)
    #define TARGET_VELOCITY_INDEX   0x60FF // 4 byte (int32_t)
    #define ACTUAL_VELOCITY_INDEX   0x606C // 4 byte (int32_t)
    #define SERVO_CTRL_WORD_INDEX   0x6040 // 2 byte (unt16_t)
    #define SERVO_STATUS_WORD_INDEX 0x6041 // 2 byte (uint16_t)
    #define ACTUAL_CURRENT_INDEX    0x6078 // 2 byte (int16_t)
    #define STOP_OPTION_INDEX       0x605A // 2 byte (uint16_t)

    class CANOPEN
    {
    private:
        std::shared_ptr<CANStream> can;
        std::vector<CanStamped> frames0; // USB_CAN ch0
        std::vector<CanStamped> frames1; // USB_CAN ch1
        int do_write(int ch, uint8_t slave, uint16_t index, uint8_t subindex);
        void parse_recv_sdo(uint16_t index, uint8_t subindex, CanStamped *p_recv_frame, uint16_t cmd);
        int8_t _servo_work_mode = 0;
        int8_t _valid_work_mode = 0;
        uint16_t _current_ctrl_word = 0; 
        uint16_t _current_status_word = 0;
        int16_t _actual_current = 0; 
        int32_t _profiled_vel = 0;
        uint32_t _profiled_acc = 0;
        uint32_t _profiled_dec = 0;
        int32_t _target_velocity = 0;
        int32_t _actual_velocity = 0;
        int32_t _target_pos = 0;
        int32_t _actual_pos = 0;
        uint16_t _stop_option = 0;
        bool _is_parsed_ok = false;

    public:
        CANOPEN(std::shared_ptr<CANStream> _can) : can(_can) {}
        template <class T>
        int ContrustWrite(CanStamped *frame, uint8_t slave, uint16_t index, uint8_t subindex, T t, uint8_t cmd = 0)
        {
            uint8_t *data = frame[0].Data;
            frame[0].ID = slave + 0x600;
            frame[0].DataLen = 8;
            memset(data, 0, 8);
            if (cmd == 0)
            {
                uint8_t cmd[] = {0x2f, 0x2b, 0x27, 0x23};
                data[0] = cmd[sizeof(T) - 1];
            }
            else
                data[0] = cmd;
            data[1] = index % 0x0100;
            data[2] = index / 0x0100;
            data[3] = subindex;
            memcpy(data + 4, &t, sizeof(T));

            return 0;
        }
        int ContrustRead(CanStamped *frame, uint8_t slave, uint16_t index, uint8_t subindex)
        {
            uint8_t *data = frame[0].Data;
            frame[0].ID = slave + 0x600;
            frame[0].DataLen = 8;
            memset(data, 0, 8);

            data[0] = 0x40;
            data[1] = index % 0x0100;
            data[2] = index / 0x0100;
            data[3] = subindex;
            return 0;
        }
        template <class T>
        int write(int ch, uint8_t slave, uint16_t index, uint8_t subindex, T t, uint8_t cmd = 0)
        {
            if (ch == 0) {
                frames0.resize(1);
                ContrustWrite<T>(&frames0[0], slave, index, subindex, t, cmd);
            }
            else if (ch == 1)
            {
                frames1.resize(1);
                ContrustWrite<T>(&frames1[0], slave, index, subindex, t, cmd);
            }
            return do_write(ch, slave, index, subindex);
        }
        void read(int ch, uint8_t slave, uint16_t index, uint8_t subindex)
        {
            if (ch == 0) {
                ContrustRead(&frames0[0], slave, index, subindex);
            }
            else if (ch == 1) {
                ContrustRead(&frames1[0], slave, index, subindex);
            }
            do_write(ch, slave, index, subindex);
        }

        inline int8_t get_servo_work_mode() { return _servo_work_mode; }
        inline int8_t get_valid_work_mode() { return _valid_work_mode; }
        inline uint16_t get_current_ctrl_word() { return _current_ctrl_word; } 
        inline uint16_t get_current_status_word() { return _current_status_word; }
        inline int16_t get_actual_current() { return _actual_current; } 
        inline uint32_t get_profiled_vel() { return _profiled_vel; }
        inline uint32_t get_profiled_acc() { return _profiled_acc; }
        inline uint32_t get_profiled_dec() { return _profiled_dec; }
        inline int32_t get_target_velocity() { return _target_velocity; }
        inline int32_t get_actual_velocity() { return _actual_velocity; }
        inline int32_t get_target_pos() { return _target_pos; }
        inline int32_t get_actual_pos() { return _actual_pos; }
        inline uint16_t get_stop_option() { return _stop_option; }
        inline void clr_parse_status() { _is_parsed_ok = false; }
        inline bool get_parse_status() { return _is_parsed_ok; }
    };
}